#include "Bounds.h"
#include "Vector3.h"
#include "Matrix3.h"
#include "Math.h"
#include "Sphere.h"
#include <assert.h>
namespace Core
{
	rtBounds rtBounds::bound_zero(rtVector3::vec3_zero, rtVector3::vec3_zero);

	inline rtBounds::rtBounds(void) 
	{
	}

	inline rtBounds::rtBounds(const rtVector3& mins, const rtVector3& maxs) 
	{
		m_Bound[0] = mins;
		m_Bound[1] = maxs;
	}

	inline rtBounds::rtBounds(const rtVector3& point) 
	{
		m_Bound[0] = point;
		m_Bound[1] = point;
	}

	inline const rtVector3& rtBounds::operator [] (const int index) const 
	{
		return m_Bound[index];
	}

	inline rtVector3& rtBounds::operator [] (const int index) 
	{
		return m_Bound[index];
	}

	inline rtBounds rtBounds::operator + (const rtVector3& t) const 
	{
		return rtBounds(m_Bound[0] + t, m_Bound[1] + t);
	}

	inline rtBounds& rtBounds::operator += (const rtVector3& t) 
	{
		m_Bound[0] += t;
		m_Bound[1] += t;
		return *this;
	}

	inline rtBounds rtBounds::operator * (const rtMatrix3& r) const 
	{
		rtBounds bound;
		bound.fromTransformedBounds(*this, rtVector3::vec3_origin, r);
		return bound;
	}

	inline rtBounds& rtBounds::operator *= (const rtMatrix3& r) 
	{
		this->fromTransformedBounds(*this, rtVector3::vec3_origin, r);
		return *this;
	}

	inline rtBounds rtBounds::operator + (const rtBounds& a) const 
	{
		rtBounds newBound;
		newBound = *this;
		newBound.addBounds(a);
		return newBound;
	}

	inline rtBounds& rtBounds::operator+=(const rtBounds& a) 
	{
		rtBounds::addBounds(a);
		return *this;
	}

	inline rtBounds rtBounds::operator-(const rtBounds& a) const 
	{
		assert(m_Bound[1][0] - m_Bound[0][0] > a.m_Bound[1][0] - a.m_Bound[0][0] &&
			   m_Bound[1][1] - m_Bound[0][1] > a.m_Bound[1][1] - a.m_Bound[0][1] &&
			   m_Bound[1][2] - m_Bound[0][2] > a.m_Bound[1][2] - a.m_Bound[0][2]);
		return rtBounds(rtVector3(m_Bound[0][0] + a.m_Bound[1][0], m_Bound[0][1] + a.m_Bound[1][1], m_Bound[0][2] + a.m_Bound[1][2]),
			           rtVector3(m_Bound[1][0] + a.m_Bound[0][0], m_Bound[1][1] + a.m_Bound[0][1], m_Bound[1][2] + a.m_Bound[0][2]));
	}

	inline rtBounds& rtBounds::operator-=(const rtBounds& a) 
	{
		assert(m_Bound[1][0] - m_Bound[0][0] > a.m_Bound[1][0] - a.m_Bound[0][0] && 
			   m_Bound[1][1] - m_Bound[0][1] > a.m_Bound[1][1] - a.m_Bound[0][1] && 
			   m_Bound[1][2] - m_Bound[0][2] > a.m_Bound[1][2] - a.m_Bound[0][2]);
		m_Bound[0] += a.m_Bound[1];
		m_Bound[1] += a.m_Bound[0];
		return *this;
	}

	inline bool rtBounds::compare(const rtBounds& a) const 
	{
		return (m_Bound[0].compare(a.m_Bound[0]) &&  m_Bound[1].compare(a.m_Bound[1]));
	}

	inline bool rtBounds::compare(const rtBounds& a, const float epsilon) const 
	{
		return (m_Bound[0].compare(a.m_Bound[0], epsilon) &&  m_Bound[1].compare(a.m_Bound[1], epsilon));
	}

	inline bool rtBounds::operator == (const rtBounds& a) const 
	{
		return compare(a);
	}

	inline bool rtBounds::operator != (const rtBounds& a) const 
	{
		return !compare(a);
	}

	inline void rtBounds::clear(void) 
	{
		m_Bound[0][0] = m_Bound[0][1] = m_Bound[0][2] = rtMath::INFINITY;
		m_Bound[1][0] = m_Bound[1][1] = m_Bound[1][2] = -rtMath::INFINITY;
	}

	inline void rtBounds::zero(void) 
	{
		m_Bound[0][0] = m_Bound[0][1] = m_Bound[0][2] =
			m_Bound[1][0] = m_Bound[1][1] = m_Bound[1][2] = 0;
	}

	inline rtVector3 rtBounds::getCenter(void) const 
	{
		return rtVector3((m_Bound[1][0] + m_Bound[0][0]) * 0.5f, (m_Bound[1][1] + m_Bound[0][1]) * 0.5f, (m_Bound[1][2] + m_Bound[0][2]) * 0.5f);
	}

	inline float rtBounds::getVolume(void) const 
	{
		if (m_Bound[0][0] >= m_Bound[1][0] || m_Bound[0][1] >= m_Bound[1][1] || m_Bound[0][2] >= m_Bound[1][2]) 
		{
			return 0.0f;
		}
		return ((m_Bound[1][0] - m_Bound[0][0]) * (m_Bound[1][1] - m_Bound[0][1]) * (m_Bound[1][2] - m_Bound[0][2]));
	}

	inline bool rtBounds::isCleared(void) const 
	{
		return m_Bound[0][0] > m_Bound[1][0];
	}

	inline bool rtBounds::addPoint(const rtVector3& v) 
	{
		bool expanded = false;
		if (v[0] < m_Bound[0][0]) 
		{
			m_Bound[0][0] = v[0];
			expanded = true;
		}
		if (v[0] > m_Bound[1][0]) 
		{
			m_Bound[1][0] = v[0];
			expanded = true;
		}
		if (v[1] < m_Bound[0][1]) 
		{
			m_Bound[0][1] = v[1];
			expanded = true;
		}
		if (v[1] > m_Bound[1][1]) 
		{
			m_Bound[1][1] = v[1];
			expanded = true;
		}
		if (v[2] < m_Bound[0][2]) 
		{
			m_Bound[0][2] = v[2];
			expanded = true;
		}
		if (v[2] > m_Bound[1][2]) 
		{
			m_Bound[1][2] = v[2];
			expanded = true;
		}
		return expanded;
	}

	inline bool rtBounds::addBounds(const rtBounds& a) 
	{
		bool expanded = false;
		if (a.m_Bound[0][0] < m_Bound[0][0]) 
		{
			m_Bound[0][0] = a.m_Bound[0][0];
			expanded = true;
		}
		if (a.m_Bound[0][1] < m_Bound[0][1]) 
		{
			m_Bound[0][1] = a.m_Bound[0][1];
			expanded = true;
		}
		if (a.m_Bound[0][2] < m_Bound[0][2]) 
		{
			m_Bound[0][2] = a.m_Bound[0][2];
			expanded = true;
		}
		if (a.m_Bound[1][0] > m_Bound[1][0]) 
		{
			m_Bound[1][0] = a.m_Bound[1][0];
			expanded = true;
		}
		if (a.m_Bound[1][1] > m_Bound[1][1]) 
		{
			m_Bound[1][1] = a.m_Bound[1][1];
			expanded = true;
		}
		if (a.m_Bound[1][2] > m_Bound[1][2]) 
		{
			m_Bound[1][2] = a.m_Bound[1][2];
			expanded = true;
		}
		return expanded;
	}

	inline rtBounds rtBounds::intersect(const rtBounds& a) const 
	{
		rtBounds n;
		n.m_Bound[0][0] = (a.m_Bound[0][0] > m_Bound[0][0]) ? a.m_Bound[0][0] : m_Bound[0][0];
		n.m_Bound[0][1] = (a.m_Bound[0][1] > m_Bound[0][1]) ? a.m_Bound[0][1] : m_Bound[0][1];
		n.m_Bound[0][2] = (a.m_Bound[0][2] > m_Bound[0][2]) ? a.m_Bound[0][2] : m_Bound[0][2];
		n.m_Bound[1][0] = (a.m_Bound[1][0] < m_Bound[1][0]) ? a.m_Bound[1][0] : m_Bound[1][0];
		n.m_Bound[1][1] = (a.m_Bound[1][1] < m_Bound[1][1]) ? a.m_Bound[1][1] : m_Bound[1][1];
		n.m_Bound[1][2] = (a.m_Bound[1][2] < m_Bound[1][2]) ? a.m_Bound[1][2] : m_Bound[1][2];
		return n;
	}

	inline rtBounds& rtBounds::intersectSelf(const rtBounds& a) 
	{
		if (a.m_Bound[0][0] > m_Bound[0][0]) 
		{
			m_Bound[0][0] = a.m_Bound[0][0];
		}
		if (a.m_Bound[0][1] > m_Bound[0][1]) 
		{
			m_Bound[0][1] = a.m_Bound[0][1];
		}
		if (a.m_Bound[0][2] > m_Bound[0][2]) 
		{
			m_Bound[0][2] = a.m_Bound[0][2];
		}
		if (a.m_Bound[1][0] < m_Bound[1][0]) 
		{
			m_Bound[1][0] = a.m_Bound[1][0];
		}
		if (a.m_Bound[1][1] < m_Bound[1][1]) 
		{
			m_Bound[1][1] = a.m_Bound[1][1];
		}
		if (a.m_Bound[1][2] < m_Bound[1][2]) 
		{
			m_Bound[1][2] = a.m_Bound[1][2];
		}
		return *this;
	}

	inline rtBounds rtBounds::expand(const float d) const 
	{
		return rtBounds(rtVector3(m_Bound[0][0] - d, m_Bound[0][1] - d, m_Bound[0][2] - d),
			           rtVector3(m_Bound[1][0] + d, m_Bound[1][1] + d, m_Bound[1][2] + d));
	}

	inline rtBounds& rtBounds::expandSelf(const float d) 
	{
		m_Bound[0][0] -= d;
		m_Bound[0][1] -= d;
		m_Bound[0][2] -= d;
		m_Bound[1][0] += d;
		m_Bound[1][1] += d;
		m_Bound[1][2] += d;
		return *this;
	}

	inline rtBounds rtBounds::translate(const rtVector3& translation) const 
	{
		return rtBounds(m_Bound[0] + translation, m_Bound[1] + translation);
	}

	inline rtBounds& rtBounds::translateSelf(const rtVector3& translation) 
	{
		m_Bound[0] += translation;
		m_Bound[1] += translation;
		return *this;
	}

	inline rtBounds rtBounds::rotate(const rtMatrix3& rotation) const 
	{
		rtBounds bound;
		bound.fromTransformedBounds(*this, rtVector3::vec3_origin, rotation);
		return bound;
	}

	inline rtBounds& rtBounds::rotateSelf(const rtMatrix3& rotation) 
	{
		fromTransformedBounds(*this, rtVector3::vec3_origin, rotation);
		return *this;
	}

	inline bool rtBounds::containsPoint(const rtVector3& p) const 
	{
		if (p[0] < m_Bound[0][0] || p[1] < m_Bound[0][1] || p[2] < m_Bound[0][2]
		|| p[0] > m_Bound[1][0] || p[1] > m_Bound[1][1] || p[2] > m_Bound[1][2]) {
			return false;
		}
		return true;
	}

	inline bool rtBounds::intersectsBounds(const rtBounds& a) const 
	{
		if (a.m_Bound[1][0] < m_Bound[0][0] || a.m_Bound[1][1] < m_Bound[0][1] || a.m_Bound[1][2] < m_Bound[0][2] || 
			a.m_Bound[0][0] > m_Bound[1][0] || a.m_Bound[0][1] > m_Bound[1][1] || a.m_Bound[0][2] > m_Bound[1][2]) 
		{
			return false;
		}
		return true;
	}

	inline rtSphere rtBounds::toSphere(void) const 
	{
		rtSphere sphere;
		sphere.setOrigin((m_Bound[0] + m_Bound[1]) * 0.5f);
		sphere.setRadius((m_Bound[1] - sphere.getOrigin()).length());
		return sphere;
	}

	inline void rtBounds::axisProjection(const rtVector3& dir, float& min, float& max) const 
	{
		float d1, d2;
		rtVector3 center, extents;

		center = (m_Bound[0] + m_Bound[1]) * 0.5f;
		extents = m_Bound[1] - center;

		d1 = dir * center;
		d2 = rtMath::Fabs(extents[0] * dir[0]) +
			 rtMath::Fabs(extents[1] * dir[1]) +
			 rtMath::Fabs(extents[2] * dir[2]);

		min = d1 - d2;
		max = d1 + d2;
	}

	inline void rtBounds::axisProjection(const rtVector3& origin, const rtMatrix3& axis, const rtVector3& dir, float& min, float& max) const
	{
		float d1, d2;
		rtVector3 center, extents;

		center = (m_Bound[0] + m_Bound[1]) * 0.5f;
		extents = m_Bound[1] - center;
		center = origin + center * axis;

		d1 = dir * center;
		d2 = rtMath::Fabs(extents[0] * (dir * axis[0])) +
			 rtMath::Fabs(extents[1] * (dir * axis[1])) +
			 rtMath::Fabs(extents[2] * (dir * axis[2]));

		min = d1 - d2;
		max = d1 + d2;
	}
}
